#include "stdafx.h"
#include "Emulator.h"
#include "CustomWorld\CustomWorld.h"

namespace ModelExamples {

  void StringConvertor(  System::String ^ sIn, // input - managed string
                         std::string & sOut // output - unmanaged string
                         )
  {
    // Marshal the managed string to unmanaged memory
    char * stringPtr = (char *) System::Runtime::InteropServices::Marshal::StringToHGlobalAnsi( sIn ).ToPointer();

    // Deep-copy to std::string
    sOut = std::string( stringPtr );

    // Free the temporary unmanaged string memory
    System::Runtime::InteropServices::Marshal::FreeHGlobal( System::IntPtr(stringPtr) );
  }


    Emulator::Emulator(System::String^ strCabPath)
      :pWorld(NULL),
      pScene(NULL),
      pShip(NULL),
      _initialTrim(0),
      _initialZ(0),
      _initialTa(0),
      _initialTf(0)
    {
      std::string str;
      StringConvertor(strCabPath, str);

      pWorld = CreateWorld();
      Depth = ToolkitWorld::DEEP_WATER_DEPTH;
      pScene = CreateModelScene(pWorld);
      pShip = pScene->AddShip(str.c_str());

      if ( pShip == NULL )
        throw std::exception("Failed to create the ship model.");
    }

    void Emulator::Start(double dblStartTimeInSecond)
    {
      double dblCourse=0;

      _startTimeInSecond  = dblStartTimeInSecond;
      _curTimeInSecond = _startTimeInSecond;

      ModelState state;
      double dblZeroOrder = 0;
      pShip->Init(0, 0, 0, dblZeroOrder);
      pShip->GetState(state);
      _initialZ = state.mZg;
      _initialTrim = state.degPitch;
      _initialTf = CalcDraft(true);
      _initialTa = CalcDraft(false);

      pShip->Init(0, 0, 0, MAX_TEL_ORDER);
      pShip->AutoCourse(dblCourse);
    }

    void Emulator::Run(double dblCurrentTimeInSecond)
    {
      double delta = dblCurrentTimeInSecond - _curTimeInSecond;
      if(delta < 0.1) return;
      double dblCurrentSceneTimeInSecond;

       pScene->Step(delta, dblCurrentSceneTimeInSecond);

      _curTimeInSecond = dblCurrentTimeInSecond;
    }
    
    EResult Emulator::SetTelegraphOrder(double dblOrder)
    {
      IModel* ship = pShip;
      unsigned int nEngines = ship->GetEnginesCount();
      if ( nEngines < 1 )
        return MR_E_FAIL;
      double* orders = new double [nEngines];
      for (unsigned int i = 0; i < nEngines; i++)
        orders[i] = dblOrder;
      EResult er = ship->SetEngineTelegraphOrders(orders, nEngines);
      delete [] orders;
      return er;
    }

    double Emulator::GetTelegraphOrder(bool returnState)
    {
      double dblOrder;
      IModel* ship = pShip;
      unsigned int nEngines = ship->GetEnginesCount();
      if ( nEngines < 1 )
        return double::NaN;
      EngineState* states = new EngineState [nEngines];
      EResult er = ship->GetEngineStates(states, nEngines);
      dblOrder = returnState ? 
        states[0].pctTelegraphState : states[0].pctTelegraphOrder;
      delete [] states;
      return dblOrder;
    }

    double Emulator::CalcDraft(bool isBow)
    {
      IomComponentPtr pRoot;
      pShip->GetIomModel(&pRoot);

      IomComponentPtr position;
      IomComponentPtr dimInfo;
      pRoot->get_Component( cidBodyFixedPosition, &position);
      pRoot->get_Component( cidDimensionInfo, &dimInfo);

      double trim =  CDataValuePtr::GetValue( dimInfo, pidTrim );

      ModelState state;
      pShip->GetState(state); 
      double Zg = state.mZg;

      double pitch  = CDataValuePtr::GetValue( position, pidFullPitch).operator[](0);
      pitch	-= _initialTrim * System::Math::PI / 180.0;

      double Ta  = CDataValuePtr::GetValue( dimInfo, pidDraftAstern);
      double Tf  = CDataValuePtr::GetValue( dimInfo, pidDraftForward);
      double Lwl = CDataValuePtr::GetValue( dimInfo, pidLengthWaterline);

      if( isBow ) 
        return Tf - Lwl/2*sin(pitch) - Zg;
      else
        return Ta + Lwl/2*sin(pitch) - Zg;                
    };

    double Emulator::CalcPitch()
    {
      IomComponentPtr pRoot;
      pShip->GetIomModel(&pRoot);

      IomComponentPtr position;
      pRoot->get_Component( cidBodyFixedPosition, &position);

      double pitch  = CDataValuePtr::GetValue( position, pidFullPitch).operator[](0);
      double pitchDeg = pitch * 180 / System::Math::PI;
      return pitchDeg;                
    };
}